package novemberBot;

import novemberBot.communication.ConnectionManager;
import novemberBot.utils.ByteArray;

public class ArmRemote {
	/**Destination byte for connection manager*/
	private final byte ARM_ID=1;
	/**reference to master brick connection manager object*/
	private ConnectionManager cm;
	
	private double clawAngle=0, clawHeight=0; 
	
	/**ArmRemote Constructor
	 * @param cm - a reference to the connection manager for this brick*/
	public ArmRemote(ConnectionManager cm){
		this.cm=cm;
	}
	/**opens or closes the claw, uses default open speed
	 * @param angle - absolute angle in deg the sides of the claw should rotate to*/
	public void openClawTo(double angle, boolean immediateReturn){
		byte[] data = new byte[5];
		byte[] bAngle=ByteArray.fromInt((int)(angle*100));
		data[0]=0;
		for(int i=0; i<4; i++){
			data[i+1]=bAngle[i];
		}
		cm.passRaw(data, ARM_ID);
		if(!immediateReturn){//fake
			try {Thread.sleep((long) (1200/Arm.OPEN_SPEED*Math.abs(clawAngle-angle))); }catch(InterruptedException e) {}
		}
		clawAngle=angle;
	}
	
	/**opens or closes the claw 
	 * @param angle - absolute angle in deg the sides of the claw should rotate to
	 * @param speed - speed in deg/s of the variation of the angle parameter*/
	public void openClawTo(double angle, double speed, boolean immediateReturn){
		byte[] data = new byte[9];
		byte[] bAngle=ByteArray.fromInt((int)angle*100);
		byte[] bSpeed=ByteArray.fromInt((int)speed*100);
		data[0]=0;
		for(int i=0; i<4; i++){
			data[i+1]=bAngle[i];
			data[i+5]=bSpeed[i];
		}
		cm.passRaw(data, ARM_ID);
		if(!immediateReturn){//fake
			try {Thread.sleep((long) (1200/speed*Math.abs(clawAngle-angle))); }catch(InterruptedException e) {}
		}
		clawAngle=angle;
		
	}
	
	
	/**used to lift and lower the claw, uses default lift speed
	 * @param height - absolute height in cm the claw should go to*/
	public void liftClawTo(double height, boolean immediateReturn){
		byte[] data = new byte[5];
		byte[] bHeight=ByteArray.fromInt((int)height*100);
		data[0]=1;
		for(int i=0; i<4; i++){
			data[i+1]=bHeight[i];
		}
		cm.passRaw(data, ARM_ID);
		if(!immediateReturn){//fake
			try {Thread.sleep((long) (1200/Arm.LIFT_SPEED*Math.abs(clawHeight-height)*Arm.HEIGHT_TO_DEG)); }catch(InterruptedException e) {}
		}
		clawHeight=height;
	}
	
	
	/**used to lift and lower the claw
	 * @param height - absolute height in cm the claw should go to
	 * @param speed - speed in cm/s of the variation of the height parameter*/
	public void liftClawTo(double height, double speed, boolean immediateReturn){
		byte[] data = new byte[9];
		byte[] bHeight=ByteArray.fromInt((int)height*100);
		byte[] bSpeed=ByteArray.fromInt((int)speed*100);
		data[0]=1;
		for(int i=0; i<4; i++){
			data[i+1]=bHeight[i];
			data[i+5]=bSpeed[i];
		}
		cm.passRaw(data, ARM_ID);
		if(!immediateReturn){//fake
			try {Thread.sleep((long) (1200/speed*Math.abs(clawHeight-height)*Arm.HEIGHT_TO_DEG)); }catch(InterruptedException e) {}
		}
		clawHeight=height;
	}
}
